#include "KalmanState.h"

#include <iostream>
#include "PDController.h"
#include "DefendingState.h"

KalmanState::KalmanState()
{
}

KalmanState::KalmanState(KalmanTank* enemy){
	this->enemy = enemy;
	outOfRange = false;

	enemy->ResetEx();
}


KalmanState::~KalmanState(void)
{
}

Command KalmanState::GetCommand(Agent* agent){
	/*otank_t* en = enemy->GetStruct();

	if(en->status.compare("dead") == 0 || outOfRange){
		outOfRange = false;
		
		agent->setState(new DefendingState());
		return agent->GetCommand();
	}*/

	enemy = GetClosestTank(agent);

	if(enemy == NULL){
		agent->setState(new DefendingState(), false);
		return agent->GetCommand();
	}

	//Vector2 aim = enemy->Predict(calcShot(agent->getTank()));//calcShot(agent->getTank());
	Vector2 aim = enemy->GetCurrentEstimate();
	//Vector2 aim = enemy->Predict(1.0);
	Vector2 pos = Vector2(agent->getTank()->pos[0], agent->getTank()->pos[1]);

	Command cmd = PDController::instance()->GetCommand(agent->getTank(), aim-pos);
	
	if(cmd.angvel() < .015)
		cmd.shoot(true);
	
	cmd.angvel(cmd.angvel()*1.7);
	cmd.speed(0);
	return cmd;
}

/*Vector2 KalmanState::calcShot(tank_t* tank){
	float t = 0.0;
	Vector2 tankP = Vector2(tank->pos[0], tank->pos[1]);
	Vector2 predicted;
	float shotV = World::instance()->getBulletSpeed();
	float shotT = 0.0;

	while(1){
		t += PREDICT_STEP;
		predicted = enemy->Predict(t);
		shotT = (predicted-tankP).magnitude()/shotV;
		if(fabs(shotT-t) < EPSILON || 
		   fabs(predicted.x) * 2.0f > World::instance()->getSize() ||
		   fabs(predicted.y) * 2.0f > World::instance()->getSize())
			return predicted;
	}
}*/

float KalmanState::calcShot(tank_t* tank){
	float t = 0.0;
	Vector2 tankP = Vector2(tank->pos[0], tank->pos[1]);
	Vector2 predicted;
	float shotV = World::instance()->getBulletSpeed();
	float shotT = 0.0;

	while(1){
		t += PREDICT_STEP;
		predicted = enemy->Predict(t);
		shotT = (predicted-tankP).magnitude()/shotV;
		if(fabs(shotT-t) < EPSILON || 
		   fabs(predicted.x) * 2.0f > World::instance()->getSize() ||
		   fabs(predicted.y) * 2.0f > World::instance()->getSize())
			return t;
	}
}


double KalmanState::normalizeAngle(double angle) {
	if (angle > 0) {
		while (angle > 2 * M_PI) {
			angle -= 2 * M_PI;
		}
	} else {
		while (angle < 0) {
			angle += 2 * M_PI;
		}
	}

	return angle;
}

KalmanTank* KalmanState::GetClosestTank(Agent* agent){
	std::vector<KalmanTank>* enemies = World::instance()->enemiesP();
	float mindist;
	KalmanTank* ret = NULL;
	Vector2 pos(agent->getTank()->pos[0],agent->getTank()->pos[1]);
		
	for(int i = 0; i < enemies->size(); i ++){
		Vector2 enemyPos = enemies->at(i).GetCurrentEstimate();
		
		if(enemies->at(i).GetStruct()->status.compare("dead") == 0)
			continue;

		float dist = (enemyPos-pos).magnitude();
		if(dist < World::instance()->getShotRange()){
			if(ret == NULL){
				ret = &enemies->at(i);
				mindist = dist;
			}
			else if(mindist > dist){
				ret = &enemies->at(i);
				mindist = dist;
			}
		}
	}

	return ret;
}